/*=================================================================
 * getApproximations.cpp constructs matrices involved in adjustment
 *
 * Dr.Kanok Weerawong
 * 29/04/2010
 *=================================================================*/
#include "stdafx.h"
#include <iostream>
#include <stdlib.h>
#include <math.h>
#include "sofa.h"
#include "sofam.h"
using namespace std;
#include "matrixManipulation.h"

struct TimeStruct {
	double year;
	double month;
	double day;
	double hour;
	double minute;
	double second;
};
struct DimapDataStruct {
	int nRows;
	TimeStruct refTime;
	int refLine;
	double linePeriod;
	double xyLos[8];
	double yawPitchRoll[3];
	double uv[2];
	int nXYZs;
	TimeStruct *tXYZ;
	double *XArray;
	double *YArray;
	double *ZArray;
	int nQuaternions;
	TimeStruct *tQuaternion;
	double *q0Array;
	double *q1Array;
	double *q2Array;
	double *q3Array;
};

/*=================================================================
 * truncToZero(): truncate the fractional part of a double 
 * input  : 
 * return : trancated number
 *
 * http://visual-c.itags.org/visual-c-c++/141318/
 * 05/05/2010
 *=================================================================*/
double truncToZero(double value)
{
double factor = pow(10.0,0);
return floor(value * factor) / factor;
}

/*=================================================================
 * lagrange() : compute Lagrange interpolation
 * input  : 
 * return : Lagrange interpolation
 *
 * Wikipedia: http://en.wikipedia.org/wiki/Lagrange_polynomial
 * 23/04/2010
 *=================================================================*/
double lagrange(double *pos, double *val, int degree, double desiredPos)
{
   double retVal = 0;
 
   for (int i = 0; i < degree; ++i) {
      double weight = 1;
       for (int j = 0; j < degree; ++j) {
         // The i-th term has to be skipped
         if (j != i) {
            weight *= (desiredPos - pos[j]) / (pos[i] - pos[j]);
         }
      }
       retVal += weight * val[i];
   }
    return retVal;
}

/*=================================================================
 * getApproximations() : compute initial approximations of imaging
 * 						 parameters (at the center of the image)
 * input   : wstring
 * returns : the value of the string in double
 *
 * Dr.Kanok Weerawong
 * 23/04/2010
 *=================================================================*/
double *getApproximations(DimapDataStruct dimapData)
{
	// Construct rotation matrix of roll, pitch, and yaw bias
	double **mRoll = new double*[3];
	mRoll[0] = new double[3];
	mRoll[1] = new double[3];
	mRoll[2] = new double[3];
	mRoll[0][0] = 1.0;
	mRoll[0][1] = 0.0;
	mRoll[0][2] = 0.0;
	mRoll[1][0] = 0.0;
	mRoll[1][1] = cos(dimapData.yawPitchRoll[2]);
	mRoll[1][2] =-sin(dimapData.yawPitchRoll[2]);
	mRoll[2][0] = 0.0;
	mRoll[2][1] = sin(dimapData.yawPitchRoll[2]);
	mRoll[2][2] = cos(dimapData.yawPitchRoll[2]);

	double **mPitch = new double*[3];
	mPitch[0] = new double[3];
	mPitch[1] = new double[3];
	mPitch[2] = new double[3];
	mPitch[0][0] = cos(dimapData.yawPitchRoll[1]);
	mPitch[0][1] = 0.0;
	mPitch[0][2] = sin(dimapData.yawPitchRoll[1]);
	mPitch[1][0] = 0.0;
	mPitch[1][1] = 1.0;
	mPitch[1][2] = 0.0;
	mPitch[2][0] =-sin(dimapData.yawPitchRoll[1]);
	mPitch[2][1] = 0.0;
	mPitch[2][2] = cos(dimapData.yawPitchRoll[1]);

	double **mYaw = new double*[3];
	mYaw[0] = new double[3];
	mYaw[1] = new double[3];
	mYaw[2] = new double[3];
	mYaw[0][0] = cos(dimapData.yawPitchRoll[0]);
	mYaw[0][1] =-sin(dimapData.yawPitchRoll[0]);
	mYaw[0][2] = 0.0;
	mYaw[1][0] = sin(dimapData.yawPitchRoll[0]);
	mYaw[1][1] = cos(dimapData.yawPitchRoll[0]);
	mYaw[1][2] = 0.0;
	mYaw[2][0] = 0.0;
	mYaw[2][1] = 0.0;
	mYaw[2][2] = 1.0;

	double **mBias = matrixMultiplyABC(mYaw,mPitch,mRoll,3,3,3,3);

	// Construct rotation matrix of quaternions
	// Compute time at image center
	TimeStruct timeAtCenter = dimapData.refTime;
	timeAtCenter.second += dimapData.linePeriod*((dimapData.nRows/2)-dimapData.refLine);
	if (timeAtCenter.second > 60.0){
		timeAtCenter.second -= 60.0;
		timeAtCenter.minute += 1.0;
		if (timeAtCenter.minute > 60.0){
			timeAtCenter.minute -= 60.0;
			timeAtCenter.hour += 1.0;
		}
	}
	double timeSecAtCenter = timeAtCenter.hour*3600
						+ timeAtCenter.minute*60
						+ timeAtCenter.second;

	// Find the index point used in the interpolation
	int indexPoint = 0;
	double timeJ;
    for (int j = 0; j < dimapData.nQuaternions ; j++){
      timeJ = dimapData.tQuaternion[j].hour*3600 + dimapData.tQuaternion[j].minute*60 
			+ dimapData.tQuaternion[j].second;
      if (timeSecAtCenter < timeJ){
	    indexPoint = j-1;
	    break;
      } 
	}
    // Check if the value of the indexPoint makes sense.
	if (indexPoint < 4){
      cout << "ERROR! Value of IndexPoint is less than 4." << endl;
	  return NULL;
    }
    // Extract 8 points for interpolation.
    // Assume that the sampling time of each of the ephemeris data and the attitude data are the same.
	double *subTimeQuaternion = new double[8];
	double *subq0Array = new double[8];
	double *subq1Array = new double[8];
	double *subq2Array = new double[8];
	double *subq3Array = new double[8];

    for (int j = 0 ; j < 8 ; j++){
      subTimeQuaternion[j] = dimapData.tXYZ[indexPoint-4+j].hour*3600 
							+ dimapData.tXYZ[indexPoint-4+j].minute*60
							+ dimapData.tXYZ[indexPoint-4+j].second;
      subq0Array[j] = dimapData.q0Array[indexPoint-4+j];
	  subq1Array[j] = dimapData.q1Array[indexPoint-4+j];
	  subq2Array[j] = dimapData.q2Array[indexPoint-4+j];
	  subq3Array[j] = dimapData.q3Array[indexPoint-4+j];
	}
	// Compute Lagrange interpolations of quaternions
    double q0 = lagrange(subTimeQuaternion,subq0Array,8,timeSecAtCenter);
    double q1 = lagrange(subTimeQuaternion,subq1Array,8,timeSecAtCenter);
    double q2 = lagrange(subTimeQuaternion,subq2Array,8,timeSecAtCenter);
    double q3 = lagrange(subTimeQuaternion,subq3Array,8,timeSecAtCenter);

	// Construct the matrix
	double **mQuaternion = new double*[3];
	mQuaternion[0] = new double[3];
	mQuaternion[1] = new double[3];
	mQuaternion[2] = new double[3];

	mQuaternion[0][0] = 1-2*(q2*q2+q3*q3);
	mQuaternion[0][1] = 2*(q1*q2-q0*q3);
	mQuaternion[0][2] = 2*(q1*q3+q0*q2);
	mQuaternion[1][0] = 2*(q2*q1+q0*q3);
	mQuaternion[1][1] = 1-2*(q1*q1+q3*q3);
	mQuaternion[1][2] = 2*(q2*q3-q0*q1);
	mQuaternion[2][0] = 2*(q3*q1-q0*q2);
	mQuaternion[2][1] = 2*(q3*q2+q0*q1);
	mQuaternion[2][2] = 1-2*(q1*q1+q2*q2);

	// Construct rotation matrix of coordinate transformation
	// From ICRF -> ITRF using SOFA library

	// Define constants
	const double ttMinusTai = 32.184; /* seconds */
	const double gpstMinusUtc = 15.0; /* seconds */

/*   BULLETIN B 255 (IAU 2000)  30 April 2009 
 *   2009     MJD       	x        y     UT1-UTC 	UT1-UT1R  	 D    	dPsi 	dEpsilon
 *    (0 h UTC)     	"        "        s       	ms     		ms  	0.001"  0.001"
 *  MAR  27 54917 	-0.11918  0.39161  0.330283 -0.063   1.633  	-59.8   -7.2
 */
	const double ut1MinusUtc = 0.330283;// seconds 

/*   Bulletin C 38, 4 July 2009 
 * from 2009 January 1, 0h UTC, until further notice : UTC-TAI = -34 s
 */
 	const double utcMinusTai = -34;// seconds

	// Compute UT1 and TT from given GPS time
	double djm0, djm1, utcSecond, ut1Second, ttSecond, ut1b, ttb;
	iauCal2jd((int)timeAtCenter.year, (int)timeAtCenter.month , (int)timeAtCenter.day, &djm0, &djm1);
    utcSecond  = timeAtCenter.second - gpstMinusUtc;
    ut1Second  = ut1MinusUtc + utcSecond;
    ttSecond   = utcSecond - utcMinusTai + ttMinusTai;
	ut1b = djm1 + (timeAtCenter.hour/24) + (timeAtCenter.minute/1440) + (ut1Second/86400);
	ttb  = djm1 + (timeAtCenter.hour/24) + (timeAtCenter.minute/1440) + (ttSecond/86400);

	// Call SOFA routines to get rotation matrix (ICRF -> ITRF)
	double xp, yp, rMatrix[3][3];
	iauXy06(djm0, ttb, &xp, &yp);
	iauC2t06a(djm0, ttb, djm0, ut1b, xp, yp, rMatrix);
	double **mIcrf2Itrf = new double*[3];
	mIcrf2Itrf[0] = rMatrix[0];
	mIcrf2Itrf[1] = rMatrix[1];
	mIcrf2Itrf[2] = rMatrix[2];

	// Compute rotation matrix from space to ground
	double **mAll = matrixMultiplyABC(mIcrf2Itrf,mQuaternion,mBias,3,3,3,3);

	// Prepare for computing Lagrange interpolation of XYZ
	// Find the index point used in the interpolation
	indexPoint = 0;
    for (int j = 0; j < dimapData.nXYZs ; j++){
      timeJ = dimapData.tXYZ[j].hour*3600 + dimapData.tXYZ[j].minute*60 + dimapData.tXYZ[j].second;
      if (timeSecAtCenter < timeJ){
	    indexPoint = j-1;
	    break;
      } 
	}
    // Check if the value of the indexPoint makes sense.
	if (indexPoint < 4){
      cout << "ERROR! Value of IndexPoint is less than 4." << endl;
	  return NULL;
    }
    // Extract 8 points for interpolation.
    // Assume that the sampling time of each of the ephemeris data and the attitude data are the same.
	double *subTimeXYZ = new double[8];
	double *subXArray = new double[8];
	double *subYArray = new double[8];
	double *subZArray = new double[8];

    for (int j = 0 ; j < 8 ; j++){
      subTimeXYZ[j] = dimapData.tXYZ[indexPoint-4+j].hour*3600 + dimapData.tXYZ[indexPoint-4+j].minute*60
					+ dimapData.tXYZ[indexPoint-4+j].second;
      subXArray[j] = dimapData.XArray[indexPoint-4+j];
	  subYArray[j] = dimapData.YArray[indexPoint-4+j];
	  subZArray[j] = dimapData.ZArray[indexPoint-4+j];
	}

	// XL, YL, ZL, omega, phi, kappa
	double *imagePar = new double[6];
    imagePar[0] = lagrange(subTimeXYZ,subXArray,8,timeSecAtCenter);
    imagePar[1] = lagrange(subTimeXYZ,subYArray,8,timeSecAtCenter);
    imagePar[2] = lagrange(subTimeXYZ,subZArray,8,timeSecAtCenter);
	// Rotation matrix has to be transposed
	imagePar[3] = atan2(-mAll[1][2],mAll[2][2]);
	imagePar[4] = asin(mAll[0][2]);
	imagePar[5] = atan2(-mAll[0][1],mAll[0][0]);

	delete [] mRoll;
	delete [] mPitch;
	delete [] mYaw;
	delete [] mQuaternion;
	delete [] subTimeQuaternion;
	delete [] subq0Array;
	delete [] subq1Array;
	delete [] subq2Array;
	delete [] subq3Array;
	delete [] mIcrf2Itrf;
	delete [] mAll;
	delete [] subTimeXYZ;
	delete [] subXArray;
	delete [] subYArray;
	delete [] subZArray;

    return imagePar;
}